﻿using Engine;
using Engine.Media;
using System;
using System.Collections.Generic;
using System.IO;

namespace LibPixz2
{
    public struct ComponentInfo
    {
        public byte id;
        public byte samplingFactorX;
        public byte samplingFactorY;
        public byte quantTableId;
        public byte dcHuffmanTable;
        public byte acHuffmanTable;
    }

    public struct QuantTable
    {
        public bool valid;
        public byte id;
        public ushort length;
        public byte precision;
        public ushort[] table;
    }

    public struct HuffmanTable
    {
        public bool valid;
        public byte id;
        public byte type;
        public byte[] numSymbols;
        public byte[] codes;
        public byte maxCodeLength;
        public List<Huffman.CodeInfo> table;
        public Huffman.CodeInfo[] preIndexTable;
    }

    public struct Info
    {
        public float a;
        public float b;
        public float c;
    }

    public struct Color2
    {
        public byte a;
        public byte r;
        public byte g;
        public byte b;
    }

    public enum Markers
    {
        LiteralFF = 0x00,
        Soi = 0xd8,
        App0 = 0xe0,
        App14 = 0xee,
        Dqt = 0xdb,
        Sof0 = 0xc0, Sof2 = 0xc2,
        Dht = 0xc4,
        Rs0 = 0xd0, Rs1 = 0xd1, Rs2 = 0xd2, Rs3 = 0xd3,
        Rs4 = 0xd4, Rs5 = 0xd5, Rs6 = 0xd6, Rs7 = 0xd7,
        Sos = 0xda,
        Eoi = 0xd9,
        Dri = 0xdd
    }

    public enum App14ColorMode
    {
        Unknown,
        YCbCr,
        YCCK
    }

    public interface IColorspaceConverter
    {
        Color2 ConvertToRgb(Info info);
        Info ConvertFromRgb(Color2 rgb);
    }

    public class ImgInfo
    {
        public ushort length;
        public byte dataPrecision;
        public ushort height;
        public ushort width;
        public bool hasRestartMarkers;
        public ushort restartInterval;
        public byte numOfComponents;
        public ComponentInfo[] components;
        public HuffmanTable[,] huffmanTables = new HuffmanTable[2, 4];
        public QuantTable[] quantTables = new QuantTable[4];
        public bool startOfImageFound;
        // For App14 Marker (Adobe)
        public bool app14MarkerFound;
        public App14ColorMode colorMode;
        // Helper image decoding variables
        public short[] deltaDc;
        //public short restartMarker;
        public int mcuStrip = 0;
        public Markers prevRestMarker = Markers.Rs7;
        // Constants
        public const int blockSize = 8;
    }

    public class Pixz
    {
        public static DynamicArray<Image> Decode(Stream stream)
        {
            var reader = new BinaryReader(stream);
            var images = new DynamicArray<Image>();
            stream.Seek(0, SeekOrigin.Begin);
            var imgInfo = new ImgInfo();
            for (long length = stream.Length; ;)
            {
                int markerId;
                do
                {
                    if (stream.Position == length)
                        goto end;
                } while (reader.ReadByte() != 0xff);
                markerId = reader.ReadByte();
                Markers markers = (Markers)markerId;
                switch (markers)
                {
                    case Markers.App0: ReadForApp0(reader, imgInfo); break;
                    case Markers.App14: ReadForApp14(reader, imgInfo); break;
                    case Markers.Dqt: ReadForDqt(reader, imgInfo); break;
                    case Markers.Sof0: ReadForSof0(reader, imgInfo); break;
                    case Markers.Dht: ReadForDht(reader, imgInfo); break;
                    case Markers.Dri: ReadForDri(reader, imgInfo); break;
                    case Markers.Sos: images.Add(ReadForSos(reader, imgInfo)); break;
                    case Markers.Soi: imgInfo = new ImgInfo { startOfImageFound = true }; break;
                    case Markers.Eoi: break;
                    case Markers.Sof2: break;
                    default: ReadForMarker(reader, imgInfo, (Markers)markerId); break;
                }
            }
        end:
            reader.Dispose();
            return images;
        }

        public static void ReadForApp0(BinaryReader reader, ImgInfo imgInfo)
        {
            ushort length = reader.ReadUInt16();
            reader.BaseStream.Seek(length - 2, SeekOrigin.Current);
        }

        public static void ReadForApp14(BinaryReader reader, ImgInfo imgInfo)
        {
            reader.ReadUInt16();
            reader.ReadBytes(11);
            imgInfo.app14MarkerFound = true;
            imgInfo.colorMode = (App14ColorMode)reader.ReadByte();
            if ((int)imgInfo.colorMode > 2) throw new Exception("Invalid Adobe colorspace");

        }

        public static void ReadForDqt(BinaryReader reader, ImgInfo imgInfo)
        {
            int markerLength = reader.ReadUInt16() - 2;
            while (markerLength > 0)
            {
                int length = ReadDqtTable(reader, imgInfo);
                markerLength -= length;
            }
        }

        public static void ReadForSof0(BinaryReader reader, ImgInfo imgInfo)
        {
            imgInfo.length = reader.ReadUInt16();
            imgInfo.dataPrecision = reader.ReadByte();
            imgInfo.height = reader.ReadUInt16();
            imgInfo.width = reader.ReadUInt16();
            imgInfo.numOfComponents = reader.ReadByte();
            if (imgInfo.length < 8) throw new Exception("Invalid length of Sof0");
            if (imgInfo.height == 0 || imgInfo.width == 0) throw new Exception("Invalid image size");
            if (imgInfo.dataPrecision != 8) throw new Exception("Unsupported data precision");
            if (imgInfo.numOfComponents != 1 && imgInfo.numOfComponents != 3) throw new Exception("Invalid number of components");
            imgInfo.components = new ComponentInfo[imgInfo.numOfComponents];
            for (int i = 0; i < imgInfo.numOfComponents; i++)
            {
                byte id = reader.ReadByte();
                if (id > 3) throw new Exception("Invalid component type");
                byte samplingFactor = reader.ReadByte();
                imgInfo.components[i].id = id;
                imgInfo.components[i].samplingFactorX = (byte)(samplingFactor >> 4);
                imgInfo.components[i].samplingFactorY = (byte)(samplingFactor & 0x0f);
                imgInfo.components[i].quantTableId = reader.ReadByte();
            }

        }

        public static void ReadForDht(BinaryReader reader, ImgInfo imgInfo)
        {
            int markerLength = reader.ReadUInt16() - 2;
            while (markerLength > 0)
            {
                int length = ReadDhtTable(reader, imgInfo);
                markerLength -= length;
            }
        }

        public static void ReadForDri(BinaryReader reader, ImgInfo imgInfo)
        {
            reader.ReadUInt16();
            ushort restartInterval = reader.ReadUInt16();
            if (restartInterval == 0) throw new Exception("Invalid restart interval (0)");
            imgInfo.restartInterval = restartInterval;
            imgInfo.hasRestartMarkers = true;
        }

        public static Image ReadForSos(BinaryReader reader, ImgInfo imgInfo)
        {
            if (imgInfo.numOfComponents != 1 && imgInfo.numOfComponents != 3)
                throw new Exception("Unsupported number of components (" +
                    imgInfo.numOfComponents + ")");
            ushort length = reader.ReadUInt16();
            byte componentsInScan = reader.ReadByte();
            for (int i = 0; i < componentsInScan; i++)
            {
                byte componentId = (byte)(reader.ReadByte() - 1);
                byte huffmanTables = reader.ReadByte();
                byte acTable = (byte)(huffmanTables & 0xf);
                byte dcTable = (byte)(huffmanTables >> 4);
                imgInfo.components[componentId].dcHuffmanTable = dcTable;
                imgInfo.components[componentId].acHuffmanTable = acTable;
            }
            reader.ReadBytes(3); // "Unused" bytes
            return ImageDecoder.DecodeImage(reader, imgInfo);
        }

        public static void ReadForMarker(BinaryReader reader, ImgInfo imgInfo, Markers markerId)
        {
            // Check if marker is not followed by a length argument
            if (markerId >= Markers.Rs0 && markerId <= Markers.Rs7)
                return;
            if (markerId == Markers.LiteralFF)
                return;
            if (!imgInfo.startOfImageFound) return;
            ushort length = reader.ReadUInt16();
            reader.BaseStream.Seek(length - 2, SeekOrigin.Current);
        }

        public static int ReadDqtTable(BinaryReader reader, ImgInfo imgInfo)
        {
            byte tableInfo = reader.ReadByte();
            byte tableId = (byte)(tableInfo & 0xf); // Low 4 bits of tableInfo
            if (tableId > 3) throw new Exception("Invalid ID for quantization table");
            var quantTable = new QuantTable
            {
                id = tableId,
                precision = (byte)(tableInfo >> 4), // High 4 bits of tableInfo
                valid = true,
                table = new ushort[64]
            };
            int sizeOfElement = quantTable.precision == 0 ? 1 : 2;
            var tabla = FileOps.tablasZigzag[8];
            // Quantizer tables are in zigzag too!
            if (quantTable.precision == 0)
            {
                for (int i = 0; i < 64; i++)
                {
                    quantTable.table[tabla[i].Y * 8 + tabla[i].X] = reader.ReadByte();
                }
            }
            else
            {
                for (int i = 0; i < 64; i++)
                {
                    quantTable.table[tabla[i].Y * 8 + tabla[i].X] = reader.ReadUInt16();
                }
            }
            imgInfo.quantTables[tableId] = quantTable;
            return 1 + 64 * sizeOfElement;
        }

        public static int ReadDhtTable(BinaryReader reader, ImgInfo imgInfo)
        {
            byte tableInfo = reader.ReadByte();
            byte tableId = (byte)(tableInfo & 0x7); // Low 3 bits of tableInfo
            int numCodes = 0;
            if (tableId > 3) throw new Exception("Invalid ID for huffman table");
            if ((tableInfo & 0xe0) != 0)  // High 3 bits of tableInfo must be zero
                throw new Exception("Invalid huffman table");
            var huffmanTable = new HuffmanTable
            {
                id = tableId,
                type = (byte)((tableInfo >> 4) & 0x1), // Bit 4 of tableInfo
                valid = true,
                numSymbols = new byte[16]
            };
            for (int i = 0; i < 16; i++)
            {
                huffmanTable.numSymbols[i] = reader.ReadByte();
                numCodes += huffmanTable.numSymbols[i];
            }
            huffmanTable.codes = new byte[numCodes];
            for (int i = 0; i < numCodes; i++)
            {
                huffmanTable.codes[i] = reader.ReadByte();
            }
            Huffman.CreateTable(ref huffmanTable);
            imgInfo.huffmanTables[huffmanTable.type, huffmanTable.id] = huffmanTable;
            return 1 + 16 + numCodes;
        }
    }

    public class FileOps
    {
        public static Dictionary<int, Point2[]> tablasZigzag = new Dictionary<int, Point2[]>()
        {
             { 8, GetZigzagTable(8, 8) },
             { 16, GetZigzagTable(16, 16) },
             { 32, GetZigzagTable(32, 32) },
             { 64, GetZigzagTable(64, 64) }
        };

        public static Point2[] GetZigzagTable(int width, int height)
        {
            if (width <= 0 || height <= 0)
                throw new Exception("Block dimensions can't be less than zero");
            var tabla = new Point2[height * width];
            int x = 0, y = 0;
            int pos = 0;
            tabla[pos++] = new Point2(x, y);
            while (pos < height * width)
            {
                if (x == width - 1)
                    tabla[pos++] = new Point2(x, ++y);
                else
                    tabla[pos++] = new Point2(++x, y);

                if (pos == height * width) break;

                while (x > 0 && y < height - 1)
                    tabla[pos++] = new Point2(--x, ++y);

                if (y == height - 1)
                    tabla[pos++] = new Point2(++x, y);
                else
                    tabla[pos++] = new Point2(x, ++y);

                if (pos == height * width) break;

                while (y > 0 && x < width - 1)
                    tabla[pos++] = new Point2(++x, --y);
            }
            return tabla;
        }

        public static void ZigZagToArray(short[] coefZig, short[,] coefDct, Point2[] order, int size)
        {
            int numElem = size * size;
            for (int i = 0; i < numElem; i++)
            {
                coefDct[order[i].Y, order[i].X] = coefZig[i];
            }
        }
    }

    public class Huffman
    {
        public struct CodeInfo
        {
            public ushort number;
            public uint code;
            public byte length;
        }

        public static void CreateTable(ref HuffmanTable huffmanTable)
        {
            ConvertToCanonicalCode(ref huffmanTable);
            PreparePreindexedTables(ref huffmanTable);
        }

        public static ushort ReadRunAmplitude(BitReader bReader, HuffmanTable table)
        {
            ushort code = bReader.Peek(table.maxCodeLength);
            CodeInfo currentCode = table.preIndexTable[code];
            bReader.Read(currentCode.length);
            return currentCode.number;
        }

        public static short ReadCoefValue(BitReader bReader, uint size)
        {
            if (size == 0) return 0;
            ushort specialBits = bReader.Read(size);
            return SpecialBitsToValue(specialBits, (short)size);
        }

        public static short SpecialBitsToValue(ushort number, short size)
        {
            int threshold = 1 << (size - 1);
            if (number < threshold)
            {
                return (short)(number - ((threshold << 1) - 1));
            }
            else
            {
                return (short)number;
            }
        }

        public static void ConvertToCanonicalCode(ref HuffmanTable huffmanTable)
        {
            int newCode = -1; // Para compensar con el incremento inicial dentro del for debajo
            uint prevLength = 1;
            uint newCodeIndex = 0;
            huffmanTable.table = new List<CodeInfo>();
            for (uint i = 0; i < huffmanTable.numSymbols.Length; i++)
            {
                uint codeLength = i + 1;
                for (uint j = 0; j < huffmanTable.numSymbols[i]; j++)
                {
                    var code = new CodeInfo();
                    int difLengths = (int)(codeLength - prevLength);
                    newCode = (newCode + 1) << difLengths;
                    prevLength = codeLength;
                    code.code = (uint)newCode;
                    code.length = (byte)codeLength;
                    code.number = huffmanTable.codes[newCodeIndex++];
                    huffmanTable.table.Add(code);
                }
            }
            huffmanTable.maxCodeLength = (byte)prevLength;
        }

        public static void PreparePreindexedTables(ref HuffmanTable huffmanTable)
        {
            // Codigo de datos preindizados
            // En esta parte, se construye una tabla de a que simbolos corresponden a cada codigo
            // tomando en cuenta los bits posteriores del codigo siguiente, ya que se lee del stream
            // un numero fijo de bits, esta misma tabla nos va a decir despues cuantos bits se van a extraer
            // de la lectura hecha anteriormente
            huffmanTable.preIndexTable = new CodeInfo[1U << huffmanTable.maxCodeLength];
            foreach (var codeWord in huffmanTable.table)
            {
                int shift = huffmanTable.maxCodeLength - codeWord.length;
                uint numCodes = 1U << shift;
                uint initialCode = codeWord.code << shift;
                for (uint nextCode = initialCode; nextCode < (numCodes + initialCode); nextCode++)
                {
                    huffmanTable.preIndexTable[nextCode] = codeWord;
                }
            }
        }

        public int[] Test()
        {
            int size = 32;
            int count = 40;
            var freqTable = new int[size];
            var rnd = new System.Random((int)DateTime.UtcNow.Ticks);
            for (int i = 0; i < size; i++)
            {
                freqTable[i] += rnd.Next(count);
            }
            return freqTable;
        }
    }

    public class BitReader
    {
        public const int dataSize = sizeof(ushort) * 8;
        public const int readerSize = sizeof(byte) * 8;
        public BinaryReader reader;
        public Markers lastReadMarker;
        public uint readData;
        public int availableBits;
        public bool dataPad;
        public bool lockReading;

        public bool PastEndOfFile
        {
            get { return dataPad && availableBits <= 0; }
        }

        public BinaryReader BaseBinaryReader
        {
            get { return reader; }
        }

        public BitReader(BinaryReader reader)
        {
            Flush();
            dataPad = false;
            this.reader = reader;
        }

        public ushort Peek(uint length)
        {
            if (length > dataSize) throw new Exception("Reading too many bits");
            // If we don't have as many bits as needed, read another chunk from stream
            if (length > availableBits)
            {
                byte nextChunk = 0;
                try
                {
                    while (availableBits <= length)
                    {
                        nextChunk = ReadByteOrMarker();
                        if (lockReading) break;
                        availableBits += readerSize;
                        readData = (readData << readerSize) | nextChunk;
                    }
                }
                // If already at the end of stream, use only the remaining bits we have read
                // before. Because the number of requested bits is less than the available bits,
                // the result of the clean data has the number of missing bits as zeros appended to
                // the right, as the huffman decoding phase needs a fixed number of bits to work
                catch (EndOfStreamException)
                {
                    dataPad = true;
                }
            }
            // We move data left and right in order to get only the bits we require
            uint cleanData = readData << dataSize * 2 - availableBits;
            cleanData >>= (int)(dataSize * 2 - length);
            return (ushort)cleanData;
        }

        public ushort Read(uint length)
        {
            if (length > dataSize) throw new Exception("Reading too many bits");
            ushort data = Peek(length);
            availableBits -= (int)length;
            int shift = dataSize * 2 - availableBits;
            // We move data to the left and then right in order to get only the bits we require
            readData <<= shift;
            readData >>= shift;
            return data;
        }

        public void StopReading()
        {
            if (!dataPad)
            {
                // Rewind all (whole) bytes we didn't use (including 2 from eof marker)
                // ToDo: check if this is correct
                int rewind = ((availableBits + readerSize - 1) / readerSize) + 2;
                reader.BaseStream.Seek(-rewind, SeekOrigin.Current);
            }
            Flush();
        }

        public void Flush()
        {
            availableBits = 0;
            readData = 0;
            lastReadMarker = 0;
            lockReading = false;
        }

        public byte ReadByteOrMarker()
        {
            if (!lockReading)
            {
                byte number = reader.ReadByte();
                if (number == 0xff) // Marker found
                {
                    byte markerValue = reader.ReadByte();
                    if (markerValue == 0x00) // 0xff00 is interpreted as a 0xff value
                    {
                        return number;
                    }
                    else
                    {
                        lastReadMarker = (Markers)markerValue;
                        lockReading = true;
                        return 0;
                    }
                }
                else // Not a marker, just return read value
                {
                    return number;
                }
            }
            else
            {
                return 0;
            }
        }

        public Markers SyncStreamToNextRestartMarker()
        {
            // When decoding actual image pixel data, ignore all markers except
            // restart markers, or EOI marker
            while (!((lastReadMarker >= Markers.Rs0 &&
                      lastReadMarker <= Markers.Rs7) ||
                      lastReadMarker == Markers.Eoi))
            {
                ReadByteOrMarker();
            }
            Markers tempMarker = lastReadMarker;
            Flush();
            return tempMarker;
        }

        public bool WasEoiFound()
        {
            if (lastReadMarker == Markers.Eoi && availableBits <= 0)
            {
                Flush();
                return true;
            }
            return false;
        }
    }

    public class ImageDecoder
    {
        public const int blkSize = ImgInfo.blockSize; // JPEG decoding block size is 8x8
        // Arrays used in different parts of the decoding process
        public static float[,] blockP = new float[blkSize, blkSize];
        public static short[,] coefDctQnt = new short[blkSize, blkSize];
        public static float[,] coefDct = new float[blkSize, blkSize];
        public const int DirRestartMarkerPeriod = 8;

        public static Image DecodeImage(BinaryReader reader, ImgInfo imgInfo)
        {
            // Used for reading those nasty variable length codes
            var bReader = new BitReader(reader);
            var img = new float[imgInfo.numOfComponents][,];
            imgInfo.deltaDc = new short[imgInfo.numOfComponents];
            for (int ch = 0; ch < imgInfo.numOfComponents; ch++)
                img[ch] = new float[imgInfo.height, imgInfo.width];
            // Determine the width of height of the MCU, based on the read subsampling
            // values from all channels (don't know if this is the correct way for all cases)
            // but it works for the most common ones
            var arr = imgInfo.components;
            var componentMax = arr[0];
            for (int i = 1; i < arr.Length; i++)
            {
                componentMax = new ComponentInfo()
                {
                    samplingFactorX = (byte)MathUtils.Max(componentMax.samplingFactorX, arr[i].samplingFactorX),
                    samplingFactorY = (byte)MathUtils.Max(componentMax.samplingFactorY, arr[i].samplingFactorY)
                };
            }
            int sizeMcuX = blkSize * componentMax.samplingFactorX;
            int sizeMcuY = blkSize * componentMax.samplingFactorY;
            int numMcusX = (imgInfo.width + sizeMcuX - 1) / sizeMcuX;
            int numMcusY = (imgInfo.height + sizeMcuY - 1) / sizeMcuY;
            for (int mcu = 0; mcu < numMcusX * numMcusY; mcu = NextMcuPos(imgInfo, bReader, mcu, numMcusX, numMcusY))
            {
                // X and Y coordinates of current MCU
                int mcuX = mcu % numMcusX;
                int mcuY = mcu / numMcusX;
                // Starting X and Y pixels of current MCU
                int ofsX = mcuX * sizeMcuX;
                int ofsY = mcuY * sizeMcuY;
                for (int ch = 0; ch < imgInfo.numOfComponents; ch++)
                {
                    int scaleX = componentMax.samplingFactorX / imgInfo.components[ch].samplingFactorX;
                    int scaleY = componentMax.samplingFactorY / imgInfo.components[ch].samplingFactorY;
                    for (int sy = 0; sy < imgInfo.components[ch].samplingFactorY; sy++)
                    {
                        for (int sx = 0; sx < imgInfo.components[ch].samplingFactorX; sx++)
                        {
                            DecodeBlock(bReader, imgInfo, img[ch], ch, ofsX + blkSize * sx, ofsY + blkSize * sy,
                                scaleX, scaleY);
                        }
                    }
                }
                if (bReader.PastEndOfFile) break;
            }
            Color2[,] imagen = MergeChannels(imgInfo, img);
            var bmp = new Image(imgInfo.width, imgInfo.height);
            bReader.StopReading();
            for (int y = 0; y < imgInfo.height; y++)
            {
                for (int x = 0; x < imgInfo.width; x++)
                {
                    var c = imagen[y, x];
                    var color = new Color(c.r, c.g, c.b, c.a);
                    if (MathUtils.Min(c.r, c.g, c.b) < 176)
                        bmp.SetPixel(x, y, color);
                }
            }
            return bmp;
        }

        public static int NextMcuPos(ImgInfo imgInfo, BitReader bReader, int mcu, int numMcusX, int numMcusY)
        {
            // If we are expecting a restart marker, find it in the stream,
            // reset the DC prediction variables and calculate the new MCU position
            // otherwise, just increment the position by one
            if (imgInfo.hasRestartMarkers &&
               (mcu % imgInfo.restartInterval) == imgInfo.restartInterval - 1 &&
               (mcu < numMcusX * numMcusY - 1))
            {
                Markers currRestMarker = bReader.SyncStreamToNextRestartMarker();
                if (currRestMarker == Markers.Eoi) // If EOI marker was found
                    return numMcusX * numMcusY; // Early terminate the decoding process
                int difference = currRestMarker - imgInfo.prevRestMarker;
                if (difference <= 0) difference += DirRestartMarkerPeriod;
                // For non corrupted images, difference should be always 1
                ResetDeltas(imgInfo);
                imgInfo.mcuStrip += difference;
                imgInfo.prevRestMarker = currRestMarker;
                return imgInfo.mcuStrip * imgInfo.restartInterval;
            }
            if (bReader.WasEoiFound())
            {
                return numMcusX * numMcusY;
            }
            return ++mcu;
        }

        public static Color2[,] MergeChannels(ImgInfo imgInfo, float[][,] imgS)
        {
            var img = new Color2[imgInfo.height, imgInfo.width];
            IColorspaceConverter converter;
            if (imgInfo.app14MarkerFound)
            {
                switch (imgInfo.colorMode)
                {
                    case App14ColorMode.Unknown:
                        converter = imgInfo.numOfComponents == 3 ? new Rgb() : (IColorspaceConverter)new YCbCr();
                        break;
                    case App14ColorMode.YCbCr:
                        converter = new YCbCr();
                        break;
                    case App14ColorMode.YCCK:
                        converter = new YCbCr();
                        break;
                    default:
                        converter = new Rgb();
                        break;
                }
            }
            else
            {
                converter = new YCbCr();
            }
            for (int y = 0; y < imgInfo.height; y++)
            {
                for (int x = 0; x < imgInfo.width; x++)
                {
                    Info info;
                    if (imgInfo.numOfComponents == 1) // Y
                    {
                        info.a = imgS[0][y, x];
                        info.b = 0;
                        info.c = 0;
                    }
                    else // YCbCr
                    {
                        info.a = imgS[0][y, x];
                        info.b = imgS[1][y, x];
                        info.c = imgS[2][y, x];
                    }
                    img[y, x] = converter.ConvertToRgb(info);
                }
            }
            return img;
        }

        public static void DecodeBlock(BitReader bReader, ImgInfo imgInfo, float[,] img,
            int compIndex, int ofsX, int ofsY, int scaleX, int scaleY)
        {
            int quantIndex = imgInfo.components[compIndex].quantTableId;
            short[] coefZig = GetCoefficients(bReader, imgInfo, compIndex, blkSize * blkSize);
            FileOps.ZigZagToArray(coefZig, coefDctQnt, FileOps.tablasZigzag[blkSize], blkSize);
            ImgOps.Dequant(coefDctQnt, coefDct, imgInfo.quantTables[quantIndex].table, blkSize);
            ImgOps.Fidct(coefDct, blockP, blkSize, blkSize);
            ImgOps.ResizeAndInsertBlock(imgInfo, blockP, img, blkSize, blkSize, ofsX, ofsY, scaleX, scaleY);
        }

        public static short[] GetCoefficients(BitReader bReader, ImgInfo imgInfo, int compIndex, int numCoefs)
        {
            var coefZig = new short[numCoefs];
            int acIndex = imgInfo.components[compIndex].acHuffmanTable;
            int dcIndex = imgInfo.components[compIndex].dcHuffmanTable;
            // DC coefficient
            uint runAmplitude = Huffman.ReadRunAmplitude(bReader, imgInfo.huffmanTables[0, dcIndex]);
            uint run;// = runAmplitude >> 4; // Upper nybble
            uint amplitude = runAmplitude & 0xf; // Lower nybble
            coefZig[0] = (short)(Huffman.ReadCoefValue(bReader, amplitude) + imgInfo.deltaDc[compIndex]);
            imgInfo.deltaDc[compIndex] = coefZig[0];
            // AC coefficients
            uint pos = 0;
            while (pos < blkSize * blkSize - 1)
            {
                runAmplitude = Huffman.ReadRunAmplitude(bReader, imgInfo.huffmanTables[1, acIndex]);
                // 0x00 is End of Block
                if (runAmplitude == 0x00) break;
                run = runAmplitude >> 4;
                amplitude = runAmplitude & 0xf;
                pos += run + 1;
                if (pos >= blkSize * blkSize) break;
                coefZig[pos] = Huffman.ReadCoefValue(bReader, amplitude);
            }
            return coefZig;
        }

        // If we found a restart marker, reset all DC prediction deltas, so we can
        // start anew and not depend on previous (possibly corrupted) data
        public static void ResetDeltas(ImgInfo imgInfo)
        {
            for (int i = 0; i < imgInfo.numOfComponents; i++)
            {
                imgInfo.deltaDc[i] = 0;
            }
        }
    }

    public class Rgb : IColorspaceConverter
    {
        public Color2 ConvertToRgb(Info info)
        {
            int r, g, b;
            r = (byte)Common.Clamp(info.a + 128, 0, 255);
            g = (byte)Common.Clamp(info.b + 128, 0, 255);
            b = (byte)Common.Clamp(info.c + 128, 0, 255);
            return new Color2() { a = 255, r = (byte)r, g = (byte)g, b = (byte)b };
        }

        public Info ConvertFromRgb(Color2 color)
        {
            return new Info() { a = color.r - 128, b = color.g - 128, c = color.b - 128 };
        }
    }

    public class YCbCr : IColorspaceConverter
    {
        public static float[,] mRgbYcbcr =
        {
            {  0.299f,   0.587f,   0.114f  },
            { -0.1687f, -0.3313f,  0.5f    },
            {  0.5f,    -0.4187f, -0.0813f }
        };

        public static float[,] mYcbcrRgb =
        {
            {  1f,  0f,        1.402f   },
            {  1f, -0.34414f, -0.71414f },
            {  1f,  1.772f,    0f       }
        };

        public Color2 ConvertToRgb(Info info)
        {
            byte r, g, b;
            float y = info.a + 128;
            float cb = info.b;
            float cr = info.c;
            r = (byte)Common.Clamp(y + cr * mYcbcrRgb[0, 2], 0, 255);
            g = (byte)Common.Clamp(y + cb * mYcbcrRgb[1, 1] + cr * mYcbcrRgb[1, 2], 0, 255);
            b = (byte)Common.Clamp(y + cb * mYcbcrRgb[2, 1], 0, 255);
            return new Color2() { a = 255, r = r, g = g, b = b };
        }

        public Info ConvertFromRgb(Color2 rgb)
        {
            Info yCbCr;
            // Valores YCbCr
            yCbCr.a = rgb.r * mRgbYcbcr[0, 0] + rgb.g * mRgbYcbcr[0, 1] + rgb.b * mRgbYcbcr[0, 2] - 128;
            yCbCr.b = rgb.r * mRgbYcbcr[1, 0] + rgb.g * mRgbYcbcr[1, 1] + rgb.b * mRgbYcbcr[1, 2];
            yCbCr.c = rgb.r * mRgbYcbcr[2, 0] + rgb.g * mRgbYcbcr[2, 1] + rgb.b * mRgbYcbcr[2, 2];
            //if (ycbcr.a > 255f || ycbcr.b > 255f || ycbcr.c > 255f)
            //Console.WriteLine("Valor ycbcr desbordado");
            return yCbCr;
        }
    }

    public static class Common
    {
        public static float Clamp(float num, float min, float max)
        {
            if (num < min)
                return min;
            if (num > max)
                return max;
            return num;
        }

        public static void Transpose(float[,] bloque, float[,] blTrns, int tamX, int tamY)
        {
            for (int y = 0; y < tamY; y++)
                for (int x = 0; x < tamX; x++)
                    blTrns[x, y] = bloque[y, x];
        }
    }

    public class ImgOps
    {
        public const int blkSize = ImgInfo.blockSize;
        public static float[,] coefYU = new float[blkSize, blkSize];
        public static float[,] coefUY = new float[blkSize, blkSize];
        public static float[,] coefUV = new float[blkSize, blkSize];
        public static ArraySlice<float> coefYUS = new ArraySlice<float>(coefYU);
        public static ArraySlice<float> coefUYS = new ArraySlice<float>(coefUY);
        public static ArraySlice<float> coefUVS = new ArraySlice<float>(coefUV);
        public static float[,] tCosXU = GetTablaICos(ImgInfo.blockSize);
        public static float[,] tCosYV = GetTablaICos(ImgInfo.blockSize);
        /*static Dictionary<int, int> bitsPorNum = new Dictionary<int, int>()
        {
            {1, 0}, {2, 1}, {4, 2}, {8, 3}, {16, 4}, {32, 5}, {64, 6}
        };*/
        public static int[] bitsPorNum = { 0, 0, 1, 0, 2, 0, 0, 0,
                                    3, 0, 0, 0, 0, 0, 0, 0,
                                    4, 0, 0, 0, 0, 0, 0, 0,
                                    0, 0, 0, 0, 0, 0, 0, 0,
                                    5, 0, 0, 0, 0, 0, 0, 0,
                                    0, 0, 0, 0, 0, 0, 0, 0,
                                    0, 0, 0, 0, 0, 0, 0, 0,
                                    0, 0, 0, 0, 0, 0, 0, 0,
                                    6
                                  };

        public static float[,] GetTablaICos(int tam)
        {
            float[,] tablaCosUX = new float[tam, tam];
            for (int u = 0; u < tam; u++)
            {
                for (int x = 0; x < tam; x++)
                {
                    tablaCosUX[u, x] = (float)Math.Cos((2 * x + 1) * u * Math.PI / (2 * tam));
                    tablaCosUX[u, x] *= (float)Math.Sqrt(2.0 / tam);
                    if (u == 0) tablaCosUX[u, x] /= (float)Math.Sqrt(2f);
                }
            }
            return tablaCosUX;
        }

        public static void Ifct8(ArraySlice<float> bloque, ArraySlice<float> res, float[,] tIcos)
        {
            float dc = bloque[0] * tIcos[0, 0];
            float suma02 = 0;
            suma02 += bloque[4] * tIcos[4, 0];
            float suma01 = 0;
            suma01 += bloque[2] * tIcos[2, 0];
            suma01 += bloque[6] * tIcos[6, 0];
            float suma11 = 0;
            suma11 += bloque[2] * tIcos[2, 1];
            suma11 += bloque[6] * tIcos[6, 1];
            float suma00 = 0;
            suma00 += bloque[1] * tIcos[1, 0];
            suma00 += bloque[3] * tIcos[3, 0];
            suma00 += bloque[5] * tIcos[5, 0];
            suma00 += bloque[7] * tIcos[7, 0];
            float suma10 = 0;
            suma10 += bloque[1] * tIcos[1, 1];
            suma10 += bloque[3] * tIcos[3, 1];
            suma10 += bloque[5] * tIcos[5, 1];
            suma10 += bloque[7] * tIcos[7, 1];
            float suma20 = 0;
            suma20 += bloque[1] * tIcos[1, 2];
            suma20 += bloque[3] * tIcos[3, 2];
            suma20 += bloque[5] * tIcos[5, 2];
            suma20 += bloque[7] * tIcos[7, 2];
            float suma30 = 0;
            suma30 += bloque[1] * tIcos[1, 3];
            suma30 += bloque[3] * tIcos[3, 3];
            suma30 += bloque[5] * tIcos[5, 3];
            suma30 += bloque[7] * tIcos[7, 3];
            float p00 = dc + suma02;
            float p01 = dc - suma02;
            float p10 = p00 + suma01;
            float p11 = p00 - suma01;
            float p12 = p01 + suma11;
            float p13 = p01 - suma11;
            res[0] = p10 + suma00;
            res[7] = p10 - suma00;
            res[3] = p11 + suma30;
            res[4] = p11 - suma30;
            res[1] = p12 + suma10;
            res[6] = p12 - suma10;
            res[2] = p13 + suma20;
            res[5] = p13 - suma20;
        }

        public static void Fidct(float[,] bloque, float[,] bloqueDct, int tamX, int tamY)
        {
            var bloqueS = new ArraySlice<float>(bloque);
            // Sacamos el IDCT de cada fila del bloque
            for (int y = 0; y < tamY; y++)
            {
                Ifct8(bloqueS.GetSlice(y), coefYUS.GetSlice(y), tCosXU);
            }
            Common.Transpose(coefYU, coefUY, tamX, tamY);
            // Ahora sacamos el DCT por columna de los resultados anteriores
            for (int u = 0; u < tamX; u++)
            {
                Ifct8(coefUYS.GetSlice(u), coefUVS.GetSlice(u), tCosYV);
            }
            for (int v = 0; v < tamY; v++)
            {
                for (int u = 0; u < tamX; u++)
                {
                    bloqueDct[v, u] = (float)Math.Round(coefUV[u, v]);
                }
            }
        }

        public static void Idct(float[,] bloque, float[,] bloqueDct, int tamX, int tamY)
        {
            int u, v, x, y;
            float suma, suma2;
            for (v = 0; v < tamY; v++)
            {
                for (u = 0; u < tamX; u++)
                {
                    for (y = 0, suma2 = 0; y < tamY; y++)
                    {
                        for (x = 0, suma = 0; x < tamX; x++)
                        {
                            suma += bloque[y, x] * tCosXU[x, u];
                        }
                        suma2 += suma * tCosYV[y, v];
                    }
                    bloqueDct[v, u] = (float)Math.Round(suma2);
                }
            }
        }

        public static void MostrarBordes(float[,] coefDct, int tam)
        {
            for (int y = 0; y < tam; y++)
                coefDct[y, tam - 1] = 96f;
            for (int x = 0; x < tam; x++)
                coefDct[tam - 1, x] = 96f;
        }

        public static void ResizeAndInsertBlock(ImgInfo imgInfo, float[,] block, float[,] imagen, int tamX, int tamY, int ofsX, int ofsY, int scaleX, int scaleY)
        {
            // Nearest neighbor interpolation
            // For staircase FTW
            if (ofsX < imgInfo.width && ofsY < imgInfo.height)
            {
                for (int j = 0; j < tamY; j++)
                {
                    for (int i = 0; i < tamX; i++)
                    {
                        for (int jj = 0; jj < scaleY; jj++)
                        {
                            for (int ii = 0; ii < scaleX; ii++)
                            {
                                int posYimg = j * scaleY + ofsY + jj;
                                int posXimg = i * scaleX + ofsX + ii;
                                if (posYimg < imgInfo.height && posXimg < imgInfo.width)
                                {
                                    imagen[posYimg, posXimg] = block[j, i];
                                }
                            }
                        }
                    }
                }
            }
        }

        public static void Dequant(short[,] pixQnt, float[,] coefDct, ushort[] matriz, int tam)
        {
            for (int y = 0; y < tam; y++)
            {
                for (int x = 0; x < tam; x++)
                {
                    coefDct[y, x] = pixQnt[y, x] * matriz[y * tam + x];
                }
            }
        }
    }

    public class ArraySlice<T>
    {
        public readonly T[,] arr;

        public int firstDimension;

        public ArraySlice(T[,] arr)
        {
            this.arr = arr;
        }

        public T this[int index]
        {
            get { return arr[firstDimension, index]; }
            set { arr[firstDimension, index] = value; }
        }

        public ArraySlice<T> GetSlice(int firstDimension)
        {
            this.firstDimension = firstDimension;
            return this;
        }
    }
}